#include "Game.h"
using namespace cocos2d;

CCScene* Game::scene()
{
	CCLOG("SCENE");
	// 'scene' is an autorelease object
    CCScene * scene = CCScene::create();

	// 'layer' is an autorelease object
	Game *layer = (Game*) Game::create();

	// add layer as a child to scene
	scene->addChild(layer);

    // return the scene
    return scene;
}

// on "init" you need to initialize your instance
bool Game::init()
{
	CCLOG("INIT");
	//////////////////////////////////////////////////////////////////////////
	// super init first
	//////////////////////////////////////////////////////////////////////////


	// add a "close" icon to exit the progress. it's an autorelease object
	CCMenuItemImage *pCloseItem = CCMenuItemImage::create(
										"CloseNormal.png",
										"CloseSelected.png",
										this,
										menu_selector(Game::menuCloseCallback) );
	pCloseItem->setPosition( ccp(CCDirector::sharedDirector()->getWinSize().width - 20, 20) );

	// create menu, it's an autorelease object
	CCMenu* pMenu = CCMenu::create(pCloseItem, NULL);
	pMenu->setPosition( CCPointZero );
	this->addChild(pMenu, 1);

	//////////////////////////////////////////////////////////////////////////
	// add your codes below...
	//////////////////////////////////////////////////////////////////////////

	//allow touch
	setTouchEnabled(true);

	b2Vec2 gravity = b2Vec2(0.0f, -9.81f);
	world = new b2World(gravity);
	world->SetContinuousPhysics(true);

	firstRayCastCallback = 0;
	secondRayCastCallback = 0;

	//DEBUG DRAW
	drawDebugDraw(true);

	drawIce();
	drawFloor();

	//scheduler
	schedule( schedule_selector(Game::update) );

    return true;
}

void Game::drawIce()
{
	ShapesByLevel* sbh = new ShapesByLevel();
	sbh->buildShapes();

	vector<vector<b2Vec2> > shapes = sbh->shapes;
	BodyFactory::createBody(shapes, world, b2_staticBody);
}

void Game::drawFloor()
{
	b2BodyDef bodyDef;
	bodyDef.type = b2_staticBody;
	bodyDef.position.Set(400.0f / PhysicsConstants::WORLD_SCALE, 50.0f / PhysicsConstants::WORLD_SCALE);

	b2Body* body = world->CreateBody(&bodyDef);
	
	b2PolygonShape* floor = new b2PolygonShape();
	floor->SetAsBox(200.0f / PhysicsConstants::WORLD_SCALE, 10.0f / PhysicsConstants::WORLD_SCALE);

	b2FixtureDef fixtureDef;
	fixtureDef.shape = floor;

	body->CreateFixture(&fixtureDef);
}


void Game::drawDebugDraw(bool draw)
{
	if (draw)
	{
		m_b2dDebugDraw = new GLESDebugDraw(32.0f);
		world->SetDebugDraw(m_b2dDebugDraw);

		uint32 flags = 0;
		flags += b2Draw::e_shapeBit;               // Shows the collision shape
		// Only need above bit really, add the below flag bits for more detail
		//flags += b2Draw::e_jointBit;             // Shows joints - where two box2D objects are joined (not using any joints here).
		flags += b2Draw::e_aabbBit;
		//flags += b2Draw::e_pairBit;                // draws a line when two sprites rect bounds are overlapping
		//flags += b2Draw::e_centerOfMassBit;
		m_b2dDebugDraw->SetFlags(flags);
	}
}

void Game::draw(void)
{
	CCLayer::draw();

	ccGLEnableVertexAttribs( kCCVertexAttribFlag_Position );

	kmGLPushMatrix();

	world->DrawDebugData();

	kmGLPopMatrix();

	CHECK_GL_ERROR_DEBUG();

	if (!CCPoint::CCPointEqualToPoint(lineStartingPoint, lineEndingPoint))
	{
		if (!(CCPoint::CCPointEqualToPoint(lineStartingPoint, CCPoint(-1.0f,-1.0f)) ||
		CCPoint::CCPointEqualToPoint(lineEndingPoint, CCPoint(-1.0f,-1.0f))))
		{
			ccDrawLine(CCPoint(lineStartingPoint.x, lineStartingPoint.y),
					   CCPoint(lineEndingPoint.x, lineEndingPoint.y));
		}
	}

	ccDrawCircle(GeometryUtils::convertB2Vec2ToCCPoint(b2Vec2(600.0f/PhysicsConstants::WORLD_SCALE, 280.0f/PhysicsConstants::WORLD_SCALE)), 10, 360, 60, false);

	if (firstRayCastCallback)
	{
		vector<RaycastHitPoint> drawnHitPoints = firstRayCastCallback->getHitPoints();
		if (drawnHitPoints.size() > 0)
		{
			vector<RaycastHitPoint>::iterator it;
			for (it = drawnHitPoints.begin(); it < drawnHitPoints.end(); it++)
			{
				b2Vec2 b2point = (*it).getPoint();
				ccDrawCircle(GeometryUtils::convertB2Vec2ToCCPoint(b2point), 3, 360, 60, false);
			}
		}
	}



	if (secondRayCastCallback)
	{
		vector<RaycastHitPoint> drawnHitPoints = secondRayCastCallback->getHitPoints();
		if (drawnHitPoints.size() > 0)
		{
			vector<RaycastHitPoint>::iterator it;
			for (it = drawnHitPoints.begin(); it < drawnHitPoints.end(); it++)
			{
				b2Vec2 b2point = (*it).getPoint();
				ccDrawCircle(GeometryUtils::convertB2Vec2ToCCPoint(b2point), 3, 360, 60, false);
			}
		}
		secondRayCastCallback = 0;
	}

	vector<Section>::iterator sectionIterator;
	for (sectionIterator = debugSections.begin(); sectionIterator < debugSections.end(); sectionIterator++)
	{
		b2Vec2 startPoint = (*sectionIterator).getStartPoint();
		ccDrawCircle(GeometryUtils::convertB2Vec2ToCCPoint(startPoint), 6, 360, 60, true);
		b2Vec2 endPoint = (*sectionIterator).getEndPoint();
		ccDrawCircle(GeometryUtils::convertB2Vec2ToCCPoint(endPoint), 6, 360, 60, true);
	}

//    glDisable(GL_TEXTURE_2D);
//    glDisableClientState(GL_COLOR_ARRAY);               // so polys draw properly and plain colour
//    glDisableClientState(GL_TEXTURE_COORD_ARRAY);       // doesnt seem to matter
//    glEnableClientState(GL_VERTEX_ARRAY);             // vertices are drawn with this flag set
//
//    world->DrawDebugData();
//
//    glDisableClientState(GL_VERTEX_ARRAY);
//    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
//    glEnableClientState(GL_COLOR_ARRAY);
//    glEnable(GL_TEXTURE_2D);

}

b2Fixture* Game::queryPoint(b2Vec2& point)
{
	for(b2Body *b = world->GetBodyList(); b; b = b->GetNext())
	{
		for (b2Fixture *f = b->GetFixtureList(); f; f = f->GetNext())
		{
			if (f->TestPoint(point))
				return f;
		}
	}
	return 0;
}


void Game::runSlicingProcess(CCPoint startPoint, CCPoint endPoint)
{
	firstRayCastCallback = RayCastCallback::init();
	secondRayCastCallback = RayCastCallback::init();
	world->RayCast(firstRayCastCallback, GeometryUtils::convertCCPointToB2Vec2(startPoint), GeometryUtils::convertCCPointToB2Vec2(endPoint));
	world->RayCast(secondRayCastCallback, GeometryUtils::convertCCPointToB2Vec2(endPoint), GeometryUtils::convertCCPointToB2Vec2(startPoint));
}

void Game::menuCloseCallback(CCObject* pSender)
{
    // "close" menu item clicked
    CCDirector::sharedDirector()->end();
}

void Game::ccTouchesBegan(CCSet* touches, CCEvent* event)
{
	CCLOG("ccTouchesBegan");
	slicing = false;

	CCSetIterator it;
	CCTouch* touch;
	for( it = touches->begin(); it != touches->end(); it++) 
	{
		touch = (CCTouch*)(*it);
		if (!touch)
			break;
		handleTouchBegan(touch);
	}
}

void Game::ccTouchesMoved(CCSet* touches, CCEvent* event)
{
	CCLOG("ccTouchesMoved");
	CCSetIterator it;
	CCTouch* touch;

	for( it = touches->begin(); it != touches->end(); it++) 
	{
		touch = (CCTouch*)(*it);
		if (!touch)
			break;
		handleTouchMoved(touch);
	}
}

void Game::ccTouchesEnded(CCSet* touches, CCEvent* event)
{
	CCLOG("ccTouchesEnded");
	if (slicing)
	{
		CCSetIterator it;
		CCTouch* touch;

		for( it = touches->begin(); it != touches->end(); it++) 
		{
			touch = (CCTouch*)(*it);
			if (!touch)
				break;
			handleTouchEnded(touch);
		}
	}
	

	if (slicing)
	{
		CCLog("runSlicingProcess");
		runSlicingProcess(lineStartingPoint, lineEndingPoint);
		CCLog("findSections");
		vector<Section> sections = findSections(firstRayCastCallback->getHitPoints(), secondRayCastCallback->getHitPoints());
		CCLog("splitHitFixtures");
		splitHitFixtures(sections);
	}


	/*vector<RaycastHitPoint> drawnHitPoints = firstRayCastCallback->getHitPoints();
	if (drawnHitPoints.size() > 0)
	{
		vector<RaycastHitPoint>::iterator it;
		for (it = drawnHitPoints.begin(); it < drawnHitPoints.end(); it++)
		{
			b2Vec2 b2point = (*it).getPoint();
			CCLog("1ST X:%f Y:%f", b2point.x, b2point.y);
		}
	}

	drawnHitPoints = secondRayCastCallback->getHitPoints();
	if (drawnHitPoints.size() > 0)
	{
		vector<RaycastHitPoint>::iterator it;
		for (it = drawnHitPoints.begin(); it < drawnHitPoints.end(); it++)
		{
			b2Vec2 b2point = (*it).getPoint();
			CCLog("2ND X:%f Y:%f", b2point.x, b2point.y);
		}
	}*/

	lineStartingPoint = CCPoint(-1.0f, -1.0f);
	lineEndingPoint = CCPoint(-1.0f, -1.0f);
}

void Game::handleTouchBegan(CCTouch* touch)
{
	mouseDown = true;
	CCPoint location = touch->locationInView();
	location = CCDirector::sharedDirector()->convertToGL(location);
	startX = location.x;
	startY = location.y;

	
	b2Vec2 pointToQuery = b2Vec2(startX / PhysicsConstants::WORLD_SCALE, startY / PhysicsConstants::WORLD_SCALE);

	if (queryPoint(pointToQuery))
		slicing = false;
	else
		slicing = true;

	rayCastInput.p1 = b2Vec2(startX, startY);

	lineStartingPoint = CCPoint(startX, startY);
	lineEndingPoint = CCPoint(-1.0f, -1.0f);
	
}

void Game::handleTouchMoved(CCTouch* touch)
{
	CCPoint location = touch->locationInView();
	location = CCDirector::sharedDirector()->convertToGL(location);

	lineEndingPoint = CCPoint(location.x, location.y);
}

void Game::handleTouchEnded(CCTouch* touch)
{
	CCPoint location = touch->locationInView();
	location = CCDirector::sharedDirector()->convertToGL(location);

	b2Vec2 pointToQuery = b2Vec2(startX / PhysicsConstants::WORLD_SCALE, startY / PhysicsConstants::WORLD_SCALE);

	if (queryPoint(pointToQuery))
		slicing = false;
	else
		slicing = true;

}

vector<Section> Game::findSections(vector<RaycastHitPoint> firstRayCastHitPoints, vector<RaycastHitPoint> secondRayCastHitPoints)
{
	vector<Section> sections;

	vector<RaycastHitPoint>::iterator it1;
	RaycastHitPoint firstPoint, secondPoint;
	for (it1 = firstRayCastHitPoints.begin(); it1 < firstRayCastHitPoints.end(); it1++)
	{
		firstPoint = *it1;
		vector<RaycastHitPoint>::iterator it2;
		for (it2 = secondRayCastHitPoints.begin(); it2 < secondRayCastHitPoints.end(); it2++)
		{
			secondPoint = *it2;
			if (firstPoint.getFixture() == secondPoint.getFixture())
			{
				sections.push_back(Section(firstPoint.getFixture(), firstPoint.getPoint(), secondPoint.getPoint()));
				break;
			}
		}
	}


	return sections;
}

void Game::splitHitFixtures(vector<Section> sections)
{
	vector<Section>::iterator it;
	
	vector<BlockShape*> splitDynamicShapes;

	debugSections = sections;
	for(it = sections.begin(); it < sections.end(); it++)
	{

		b2Fixture* fixture = (*it).getFixture();
		b2PolygonShape* shape = (b2PolygonShape*) fixture->GetShape();
		b2Vec2* vertices = shape->m_vertices;
		int vertexCount = shape->GetVertexCount();
		
		b2Vec2 firstPoint = (*it).getStartPoint();
		b2Vec2 secondPoint = (*it).getEndPoint();
		int firstPointPosition = findPositionInShape(vertices, vertexCount, firstPoint);
		int secondPointPosition = findPositionInShape(vertices, vertexCount, secondPoint);

		//Reorder hit points positions
		if (firstPointPosition > secondPointPosition)
		{
			int positionTmp = firstPointPosition;
			b2Vec2 pointTmp = firstPoint;
			firstPointPosition = secondPointPosition;
			firstPoint = secondPoint;
			secondPointPosition = positionTmp;
			secondPoint = pointTmp;
		}

		//Reorder points to create new shapes
		//0,...,[p1,...,p2],...,n
		int firstShapeVertexCount = secondPointPosition - firstPointPosition + 2;
		b2Vec2* firstShapeVertices = new b2Vec2[firstShapeVertexCount];
		firstShapeVertices[0] = firstPoint;
		for (int i = firstPointPosition; i < secondPointPosition; i++)
		{
			firstShapeVertices[i - firstPointPosition + 1] = vertices[i];
		}
		firstShapeVertices[secondPointPosition - firstPointPosition + 1] = secondPoint;

		//[0,...,p1],...,[p2,...,n]
		int secondShapeVertexCount = firstPointPosition - 0 + 1 + vertexCount - secondPointPosition + 1;
		b2Vec2* secondShapeVertices = new b2Vec2[secondShapeVertexCount];
		secondShapeVertices[0] = vertices[0];
		for (int i = 1; i < firstPointPosition; i++)
		{
			secondShapeVertices[i] = vertices[i];
		}
		secondShapeVertices[firstPointPosition] = firstPoint;
		secondShapeVertices[firstPointPosition + 1] = secondPoint;
		for (int i = secondPointPosition; i < vertexCount; i++)
		{
			secondShapeVertices[firstPointPosition + 2 + i - secondPointPosition] = vertices[i];
		}

		//Building shapes
		//1st
		BlockShape* firstPolygonShape = new BlockShape(false);
		
		firstPolygonShape->Set(&firstShapeVertices[0], firstShapeVertexCount);

		b2FixtureDef firstFixtureDef;
		firstFixtureDef.shape = firstPolygonShape;

		//2nd
		BlockShape* secondPolygonShape = new BlockShape(false);
		secondPolygonShape->Set(&secondShapeVertices[0], secondShapeVertexCount);
		b2FixtureDef secondFixtureDef;
		secondFixtureDef.shape = secondPolygonShape;

		//Determine which shape should be dynamic and fall
		int dynamicBody = findDynamicBody(firstPoint, secondPoint, firstPolygonShape);

		b2Body* targetBody = fixture->GetBody();
		if (dynamicBody == DYNAMIC_BODY_FIRST_SHAPE)
		{
			targetBody->CreateFixture(&secondFixtureDef);
			splitDynamicShapes.push_back(firstPolygonShape);
		}
		else
		{
			targetBody->CreateFixture(&firstFixtureDef);
			splitDynamicShapes.push_back(secondPolygonShape);
		}
		
		targetBody->DestroyFixture(fixture);
	}
	CCLog("rebuildBlocksFromShapes");
	rebuildBlocksFromShapes(splitDynamicShapes);
	CCLog("bodyCount:%i", world->GetBodyCount());
}

int Game::findDynamicBody(b2Vec2 sectionStartPoint, b2Vec2 sectionEndPoint, b2PolygonShape* firstShape)
{
	int shapeVertexCount = firstShape->GetVertexCount();
	for (int i = 0; i < shapeVertexCount; i++)
	{
		if (GeometryUtils::determinant(sectionStartPoint, sectionEndPoint, firstShape->GetVertex(i)) < 0 && sectionStartPoint.x < sectionEndPoint.x
			|| GeometryUtils::determinant(sectionStartPoint, sectionEndPoint, firstShape->GetVertex(i)) > 0 && sectionStartPoint.x > sectionEndPoint.x)
		{
			return DYNAMIC_BODY_FIRST_SHAPE;
		}
	}
	return DYNAMIC_BODY_SECOND_SHAPE;
}

int Game::findPositionInShape(b2Vec2* vertices, int vertexCount, b2Vec2 point)
{
	for (int i = 1; i < vertexCount; i++)
	{
		b2Vec2 firstVertex = vertices[i-1];
		b2Vec2 secondVertex = vertices[i];
		if (abs(GeometryUtils::determinant(firstVertex, point, secondVertex)) < 1E-2)
			return i;
	}
	return vertexCount;
}

void Game::rebuildBlocksFromShapes(vector<BlockShape*> shapes)
{
	vector<b2Body*> resultingBodies;
	int shapesSize = shapes.size();
	BlockShape* firstShape = shapes.at(0);
	resultingBodies.push_back(BodyFactory::createBody(firstShape, world, b2_dynamicBody));
	firstShape->setInsideBlock(true);
	vector<BlockShape*>::iterator shapeIterator;
	vector<b2Body*>::iterator resultingBodiesIterator;
	shapesSize--;
	if (shapesSize == 0)
	{
		shapes.clear();
		return;
	}
	int successfulLoopNumber = 0;
	int previousLoopNumber = 0;
	while (shapesSize > 0)
	{
		previousLoopNumber = successfulLoopNumber;
		for (shapeIterator = shapes.begin(); shapeIterator < shapes.end(); shapeIterator++)
		{
			if (!(*shapeIterator)->isInsideBlock())
			{
				CCLog("FOR1");
				for (resultingBodiesIterator = resultingBodies.begin(); resultingBodiesIterator < resultingBodies.end(); resultingBodiesIterator++)
				{
					CCLog("FOR2");
					b2Fixture* fixtureList = (*resultingBodiesIterator)->GetFixtureList();
					for (b2Fixture* fix = fixtureList; fix; fix = fix->GetNext())
					{
						CCLog("FOR3");
						b2PolygonShape* fixtureShape = (b2PolygonShape*) fix->GetShape();
						if (GeometryUtils::hasOneEdgeShared(fixtureShape, *shapeIterator))
						{
							CCLog("HAS ONE EDGE SHARED");
							b2FixtureDef fixtureDef;
							fixtureDef.shape = *shapeIterator;
							(*shapeIterator)->setInsideBlock(true);
							(*resultingBodiesIterator)->CreateFixture(&fixtureDef);
							shapesSize--;
							if (shapesSize == 0)
							{
								shapes.clear();
								return;
							}
							successfulLoopNumber++;
							break;
						}
					}
				}
			}
			else
				CCLog("INSIDE BLOCK");
		}
		if (previousLoopNumber == successfulLoopNumber)
		{
			CCLog("previousLoopNumber == successfulLoopNumber");
			int index = 0;
			while (shapes.at(index)->isInsideBlock())
			{
				index++;
			}
			BlockShape* freeShape = shapes.at(index);
			freeShape->setInsideBlock(true);
			resultingBodies.push_back(BodyFactory::createBody(freeShape, world, b2_dynamicBody));
			shapesSize--;
			if (shapesSize == 0)
			{
				shapes.clear();
				return;
			}
		}
	}
}


float32 RayCastCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, const b2Vec2 &normal, float32 fraction)
{
	hitPoints.push_back(RaycastHitPoint(fixture, point, normal, fraction));

	return 1;
}





void Game::update(float32 dt)
{
	//It is recommended that a fixed time step is used with Box2D for stability
	//of the simulation, however, we are using a variable time step here.
	//You need to make an informed choice, the following URL is useful
	//http://gafferongames.com/game-physics/fix-your-timestep/
	dt = 1.0f / 60.0f;
	int velocityIterations = 8;
	int positionIterations = 1;

	// Instruct the world to perform a single step of simulation. It is
	// generally best to keep the time step and iterations fixed.
	world->Step(dt, velocityIterations, positionIterations);
	
	//Iterate over the bodies in the physics world
	//for (b2Body* b = world->GetBodyList(); b; b = b->GetNext())
	//{
	//	if (b->GetUserData() != NULL) {
	//		//Synchronize the AtlasSprites position and rotation with the corresponding body
	//		CCSprite* myActor = (CCSprite*)b->GetUserData();
	//		myActor->setPosition( CCPointMake( b->GetPosition().x * PTM_RATIO, b->GetPosition().y * PTM_RATIO) );
	//		myActor->setRotation( -1 * CC_RADIANS_TO_DEGREES(b->GetAngle()) );
	//	}	
	//}
}
